// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PidControllerTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Core.Algorithms.UnitTests
{
    using System;
    using Microsoft.Robotics.Core.Algorithms;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// The PID Controller unit test class.
    /// </summary>
    [TestClass]
    public class PidControllerTests
    {
        /// <summary>
        /// Test for correct output when controlled set of measurement errors are fed in.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void CorrectOutputWhenControlledSetOfMeasurementErrorsAreFedIn()
        {
            // we set expirty to 10 ms to make sure our fake time ticks will be considered valid
            // and we initialize all 3 of PID parameters (individual parameter tests are further down)
            var pid = new PidController(
                10,         // max output
                -10,        // min output
                0.0,        // no min absolute value
                0.3,        // Kp
                5,          // Ki
                0.001,      // Kd
                10 * 1000); // 10 ms is expirty time (we'll use fake time in all tests)

            // for demo purposes, we will decompose expected output based on individual P,I,D values, i.e.
            // in the below "-0.3 + 0 + 0" example, - 0.3 is the 'P' portion of output, and two 0s are for I and D
            Assert.AreEqual(-0.3 + 0 + 0, pid.Evaluate(1, 1 * 1000)); // only P is active (no history yet for I,D to engage)

            // Note how we should increase time with every measurement (last param) to have I and D parameters be effective
            Assert.AreEqual(-0.6 - 0.01 - 1, pid.Evaluate(2, 2 * 1000));

            // we use rounding from now on due to likely errors in doing floating point math
            double output1 = pid.Evaluate(3, 3 * 1000);
            Assert.AreEqual(-0.9 - (0.01 + 0.015) - 1, Math.Round(output1, 4, MidpointRounding.AwayFromZero));

            double output2 = pid.Evaluate(1, 4 * 1000);
            Assert.AreEqual(-0.3 - (0.01 + 0.015 + 0.005) + 2, Math.Round(output2, 4, MidpointRounding.AwayFromZero));
        }

        /// <summary>
        /// Test for correct output when only proportional is used.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void CorrectOutputWhenOnly_Proportinal_IsUsed()
        {
            var pid = new PidController(10, -10, 0, 0.5, 0, 0, 0);

            Assert.AreEqual(0.0, pid.Evaluate(0, 0));

            Assert.AreEqual(-1.0, pid.Evaluate(2, 0));

            Assert.AreEqual(-2.5, pid.Evaluate(5, 0));

            Assert.AreEqual(2.5, pid.Evaluate(-5, 0));
        }

        /// <summary>
        /// Test first measurement ignored when only integral is used.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void FirstMeasurementIgnoredWhenOnly_Integral_IsUsed()
        {
            // we set expirty to 10 ms to make sure our fake time ticks will be considered valid
            var pid = new PidController(10, -10, 0, 0, 0.5, 0, 10 * 1000);

            // use fake time (in ms)
            Assert.AreEqual(0.0, pid.Evaluate(2, 1 * 1000));
        }

        /// <summary>
        /// Test correct output when only integral is used.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void CorrectOutputsWhenOnly_Integral_IsUsed()
        {
            // we set expirty to 10 ms to make sure our fake time ticks will be considered valid
            var pid = new PidController(10, -10, 0, 0, 0.5, 0, 10 * 1000);

            // use fake time (in ms). First measurement is ignored
            pid.Evaluate(2, 1 * 1000);

            double output1 = pid.Evaluate(2, 2 * 1000);
            Assert.AreEqual(-0.001, output1);

            double output2 = pid.Evaluate(5, 3 * 1000);
            Assert.AreEqual(output1 - 0.0025, output2);

            double output3 = pid.Evaluate(-10, 4 * 1000);
            Assert.AreEqual(output2 + 0.005, output3);
        }

        /// <summary>
        /// Test zero output when integral is used and time expires.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void ZeroOutputsWhen_Integral_IsUsedAndTimeExpires()
        {
            var pid = new PidController(10, -10, 0, 0.0, 0.5, 0.0, 10 * 1000);

            // use fake time (in ms). First measurement is ignored
            pid.Evaluate(2, 1 * 1000);

            Assert.AreEqual(pid.Evaluate(3, 12 * 1000), 0);

            Assert.AreEqual(pid.Evaluate(5, 23 * 1000), 0);
        }

        /// <summary>
        /// Test first measurement is ignored when only derivative is used.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void FirstMeasurementIsIgnoredWhenOnly_Derivative_IsUsed()
        {
            // we set expirty to 10 ms to make sure our fake time ticks will be considered valid
            var pid = new PidController(10, -10, 0, 0, 0, 0.5, 10 * 1000);

            // use fake time (in ms)
            Assert.AreEqual(0.0, pid.Evaluate(2, 1 * 1000));
        }

        /// <summary>
        /// Test correct output when only derivative is used.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void CorrectOutputWhenOnly_Derivative_IsUsed()
        {
            // we set expirty to 10 ms to make sure our fake time ticks will be considered valid
            var pid = new PidController(10, -10, 0, 0, 0, 0.5, 10 * 1000);

            // use fake time (in ms)
            pid.Evaluate(2, 1 * 1000);

            double output1 = pid.Evaluate(2.01, 2 * 1000);
            Assert.AreEqual(-5, Math.Round(output1, 3, MidpointRounding.AwayFromZero));

            double output2 = pid.Evaluate(2.06, 12 * 1000);
            Assert.AreEqual(-2.5, Math.Round(output2, 3, MidpointRounding.AwayFromZero));

            double output3 = pid.Evaluate(2.02, 22 * 1000);
            Assert.AreEqual(2, Math.Round(output3, 3, MidpointRounding.AwayFromZero));
        }

        /// <summary>
        /// Test zero output when only derivative is used and error is constant.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void ZeroOutputWhenOnly_Derivative_IsUsedAndErrorIsConstant()
        {
            // we set expirty to 10 ms to make sure our fake time ticks will be considered valid
            var pid = new PidController(10, -10, 0, 0, 0, 0.5, 10 * 1000);

            pid.Evaluate(3, 1 * 1000);

            Assert.AreEqual(0, pid.Evaluate(3, 2 * 1000));

            Assert.AreEqual(0, pid.Evaluate(3, 3 * 1000));
        }

        /// <summary>
        /// Test zero output when derivative is used and time expires.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void ZeroOutputsWhen_Derivative_IsUsedAndTimeExpires()
        {
            var pid = new PidController(10, -10, 0, 0.0, 0.0, 0.5, 10 * 1000);

            // use fake time (in ms). First measurement is ignored
            pid.Evaluate(2, 1 * 1000);

            Assert.AreEqual(pid.Evaluate(3, 12 * 1000), 0);

            Assert.AreEqual(pid.Evaluate(5, 23 * 1000), 0);
        }

        /// <summary>
        /// Test zero output when integral and derivative re used and time measurement is wrong.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void ZeroOutputsWhen_I_or_D_AreUsedAndTimeMeasurementIsWrong()
        {
            var pid = new PidController(10, -10, 0, 0.0, 0.5, 0.5, 10 * 1000);

            // use fake time (in ms). First measurement is ignored
            pid.Evaluate(2, 10 * 1000);

            Assert.AreEqual(pid.Evaluate(3, 9 * 1000), 0);

            Assert.AreEqual(pid.Evaluate(5, -6 * 1000), 0);
        }

        /// <summary>
        /// Test output is clamped when min/max clamping is set.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void OutputIsClampedWhen_Min_Max_ClampingsAreSet()
        {
            const double MinClamp = -15;
            const double MaxClamp = 10;

            var pid = new PidController(MaxClamp, MinClamp, 0, 0.5, 0, 0, 10);

            Assert.AreEqual(MinClamp, pid.Evaluate(100, 0));

            Assert.AreEqual(MaxClamp, pid.Evaluate(-21, 0));
        }

        /// <summary>
        /// Test output is clamped when absolute min clamping is set.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void OutputIsClampedWhen_AbsoluteMin_ClampingsAreSet()
        {
            const double MinAbsValueClamp = 2;

            var pid = new PidController(10, -10, MinAbsValueClamp, 0.5, 0, 0, 10);

            Assert.AreEqual(-MinAbsValueClamp, pid.Evaluate(1, 0));

            Assert.AreEqual(MinAbsValueClamp, pid.Evaluate(-1, 0));
        }

        /// <summary>
        /// Test output is clamped when absolute min clamping is set.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void OutputIsZeroEvenWhen_AbsoluteMin_ClampingsAreSetAndErrorIsZero()
        {
            var pid = new PidController(
                10, 
                -10,
                minAbsoluteOutput: 2,
                proportionalGain: 0.5,
                integralGain: 0,
                derivativeGain: 0,
                measurementExpiryTicks: 0);

            Assert.AreEqual(0, pid.Evaluate(measuredError: 0, measurementTimeTicksMicroseconds: 0));
        }

        /// <summary>
        /// Test custom PID parameters used when passed in directly.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void CustomPIDParamsUsedWhenPassedInDirectly()
        {
            var pid = new PidController(10, -10, 0.0, 0.3, 5, 0.001, 10 * 1000);

            // first calculation done based on PID parameters from ctor
            Assert.AreEqual(-0.3, pid.Evaluate(1, 1 * 1000));

            // second time around - we pass in PID params directly
            double output1 = pid.Evaluate(2, 2 * 1000, 0.5, 10, 0.002);
            Assert.AreEqual(-1.0 - 0.02 - 2, output1);

            // third time around, old PID params are still used (pre-set PID parameters
            // are unaffected
            double output2 = pid.Evaluate(3, 3 * 1000);
            Assert.AreEqual(-0.9 - (0.01 + 0.015) - 1, Math.Round(output2, 4, MidpointRounding.AwayFromZero));
        }

        /// <summary>
        /// Test custom PID parameters used when twiddled.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        public void CustomPIDParamsUsedWhenTwiddled()
        {
            var pid = new PidController(10, -10, 0.0, 0.3, 5, 0.001, 10 * 1000);

            // first calculation done based on PID parameters from ctor
            Assert.AreEqual(-0.3, pid.Evaluate(1, 1 * 1000));

            // second time around - we twiddle PID params and then do the evaluation
            pid.ProportionalGain = 0.5;
            pid.IntegralGain = 10;
            pid.DerivativeGain = 0.002;
            Assert.AreEqual(-1.0 - 0.02 - 2, pid.Evaluate(2, 2 * 1000));
        }
    }
}
